/*
 * ANO_driver.c
 *
 *  Created on: 2022.2.27
 *      Author: huan
 *
 *  Explain:data input with _E suffix should be multiplied by 10e-x
 */
#include "head_user.h"

/*-----------------------------------------------------------------*/
ANO_Send_t ANO_Send;                            /* External interface 1*/

static void send_data(uint8_t *buffer, uint8_t LEN) /* External interface 2 */
{
    //USER_VCOM.send((char*)buffer, LEN);
    USER_UART1.send((char*)buffer, LEN);
}
/*-----------------------------------------------------------------*/
/* 数据拆分成单字节发送 */
#define BYTE0(dat) (*((char *)(&dat)    ))
#define BYTE1(dat) (*((char *)(&dat) + 1))
#define BYTE2(dat) (*((char *)(&dat) + 2))
#define BYTE3(dat) (*((char *)(&dat) + 3))
#define bit8_convert_small      data_buffer[_cnt++]=BYTE0(_temp)
#define bit16_convert_small     data_buffer[_cnt++]=BYTE0(_temp),\
                                data_buffer[_cnt++]=BYTE1(_temp)
#define bit32_convert_small     data_buffer[_cnt++]=BYTE0(_temp),\
                                data_buffer[_cnt++]=BYTE1(_temp),\
                                data_buffer[_cnt++]=BYTE2(_temp),\
                                data_buffer[_cnt++]=BYTE3(_temp)

#define Default_bufferSize 40
#define Head_size   4
#define Send_add    2

#define HW_ALL      0xFF
#define SWJ_ADDR    0xAF    //上位机地址ַ

#define HW_TYPE     0x05    //拓空者ID
#define HW_VER      1       //固件版本
#define SW_VER      17      //软件版本
#define BL_VER      0       //BootLoader
/*-----------------------------------------------------------------*/
union Ano_HW_t
{
    struct
    {
         uint32_t reserve_0;
         uint32_t hwtype;
         uint32_t hwverion;
         uint32_t swverion;
         uint32_t blverion;
         uint32_t reserve_1;
         uint32_t reserve_2;
         uint32_t reserve_3;
         uint32_t reserve_4;
         uint32_t reserve_5;
         uint32_t reserve_6;

    };
    uint32_t HW[11];
};
static union Ano_HW_t Ano_HW;

typedef enum
{
    Broadcast = 0xFF,
    Upper = 0xAF,
    TuoKongzhe = 0x05,
    ShuChuan = 0x10,
    Guangliu = 0x22,
    UWB = 0x30,
    IMU = 0x60,
    LingXiao = 0x61
}D_ADDR_t;

typedef enum
{
    DataVerify = 0,
    Imu_adc,
    Compass_baro_temp,
    Euler_angle,
    Quaternion,
    Altitude,
    Remote = 0x40,
    Ano_CMD = 0xE0,
    Parame_updat = 0xE1,
    Parame_back = 0xE2,
}ID_t;
/*-----------------------------------------------------------------*/


/*-----------------------------------------------------------------*/
/* 帧头 */
static void ANO_data_head(D_ADDR_t D_ADDR, ID_t ID, uint8_t LEN, uint8_t *buffer)
{
    buffer[0] = 0xAA;
    buffer[1] = D_ADDR;
    buffer[2] = ID;
    buffer[3] = LEN;
}
/* ֡βУ�� */
static void ANO_data_check(uint8_t *buffer)
{
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0; i < (buffer[3]+4); i++)
    {
        sumcheck += buffer[i];
        addcheck += sumcheck;
    }
    buffer[buffer[3] + 4] = sumcheck;
    buffer[buffer[3] + 5] = addcheck;
}


/*===================================================================================================*/
/* inertial sensor data */
static uint32_t ANO_InertialData_send(int16_t ACC_X,int16_t ACC_Y,int16_t ACC_Z,
                                    int16_t GYR_X,int16_t GYR_Y,int16_t GYR_Z)
{
    uint8_t data_buffer[Default_bufferSize], _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, Imu_adc, 13, data_buffer);
    _temp = ACC_X;
    bit16_convert_small;
    _temp = ACC_Y;
    bit16_convert_small;
    _temp = ACC_Z;
    bit16_convert_small;
    _temp = GYR_X;
    bit16_convert_small;
    _temp = GYR_Y;
    bit16_convert_small;
    _temp = GYR_Z;
    bit16_convert_small;
    data_buffer[_cnt++]=0;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}
/* Compass_barometer_temperature data */
static uint32_t ANO_CompassBaroTempData_send(int16_t MAG_X,int16_t MAG_Y,int16_t MAG_Z,
                                             uint32_t ALT_BAR_E2,int16_t TMP_E2)
{
    uint8_t data_buffer[Default_bufferSize],   _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, Compass_baro_temp, 14, data_buffer);
    _temp = MAG_X;
    bit16_convert_small;
    _temp = MAG_Y;
    bit16_convert_small;
    _temp = MAG_Z;
    bit16_convert_small;
    _temp = ALT_BAR_E2;
    bit32_convert_small;
    _temp = TMP_E2;
    bit16_convert_small;
    data_buffer[_cnt++]=0;
    data_buffer[_cnt++]=0;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}
/* EulerAngle data */
static uint32_t ANO_EulerAngleData_send(int16_t ROL_E2,int16_t PIT_E2,int16_t YAW_E2)
{
    uint8_t data_buffer[Default_bufferSize], _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, Euler_angle, 7, data_buffer);
    _temp = ROL_E2;
    bit16_convert_small;
    _temp = PIT_E2;
    bit16_convert_small;
    _temp = YAW_E2;
    bit16_convert_small;
    data_buffer[_cnt++]=0;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}

/* Quaternions data */
static uint32_t ANO_QuaternionsData_send(int16_t V0_E4,int16_t V1_E4,int16_t V2_E4,int16_t V3_E4)
{
    uint8_t data_buffer[Default_bufferSize], _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, Quaternion, 9, data_buffer);
    _temp = V0_E4;
    bit16_convert_small;
    _temp = V1_E4;
    bit16_convert_small;
    _temp = V2_E4;
    bit16_convert_small;
    _temp = V3_E4;
    bit16_convert_small;
    data_buffer[_cnt++]=0;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}

/* Quaternions data */
static uint32_t ANO_RemoteData_send(int16_t ROL,int16_t PIT,int16_t THR,int16_t YAW,int16_t AUX1,int16_t AUX2,int16_t AUX3,int16_t AUX4,int16_t AUX5,int16_t AUX6)
{
    uint8_t data_buffer[Default_bufferSize], _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, Remote, 20, data_buffer);
    _temp = ROL;
    bit16_convert_small;
    _temp = PIT;
    bit16_convert_small;
    _temp = THR;
    bit16_convert_small;
    _temp = YAW;
    bit16_convert_small;
    _temp = AUX1;
    bit16_convert_small;
    _temp = AUX2;
    bit16_convert_small;
    _temp = AUX3;
    bit16_convert_small;
    _temp = AUX4;
    bit16_convert_small;
    _temp = AUX5;
    bit16_convert_small;
    _temp = AUX6;
    bit16_convert_small;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}
/*===================================================================================================*/


static void Parameter_update(uint16_t ID, int32_t value)
{
    PID_Struct *pid = PID_Handle.pid;

    if(ID < 11)                     //hardware info
    {
        return;                     //modification is not supported
    }
    else if(ID < 65)                //PID info
    {
        uint8_t x = ((ID - 11) / 3);          //number of group - 1
        uint8_t y = (ID - 11) - 3*x;          //index
        Parame_PID.byte[y*Pid_Num+x] = (float)((int32_t)value / 1000.0f);
        if(y == 0) pid[x].Kp = (int32_t)value / 1000.0f;
        if(y == 1) pid[x].Ki = (int32_t)value / 1000.0f;
        if(y == 2) pid[x].Kd = (int32_t)value / 1000.0f;
    }
    else if( ID < 167 )
    {
        Parame_PID.byte[ID-11] = (float)((int32_t)value / 1000.0f);
    }
    else {
        return;
    }
}

static int32_t ParameBack_getValue(uint16_t ID)
{
    if(ID < 11)                     //hardware info
    {
        return Ano_HW.HW[ID];       //modification is not supported
    }
    else if(ID < 65)                //PID info
    {
        uint8_t x = ((ID - 11) / 3);        //number of group - 1
        uint8_t y = (ID - 11) - 3*x;        //index
        return Parame_PID.byte[y*Pid_Num+x] * 1000;
    }
    else if( ID < 167 )
    {
        return Parame_PID.byte[ID-11] * 1000;
    }
    else {
        return 0;
    }
}

static uint32_t ANO_ParameBack(uint16_t ID)
{
    uint8_t data_buffer[Default_bufferSize], _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, Parame_back, 6, data_buffer);
    _temp = ID;
    bit16_convert_small;
    _temp = ParameBack_getValue(ID);
    bit32_convert_small;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}

static uint32_t ANO_DataVerify(uint8_t ID, uint8_t SC, uint8_t AC)
{
    uint8_t data_buffer[Default_bufferSize], _cnt = Head_size;
    uint32_t _temp;

    ANO_data_head(Broadcast, DataVerify, 3, data_buffer);
    _temp = ID;
    bit8_convert_small;
    _temp = SC;
    bit8_convert_small;
    _temp = AC;
    bit8_convert_small;
    ANO_data_check(data_buffer);

    send_data(data_buffer, _cnt + Send_add);
    return 0;
}

static uint8_t DT_RxBuffer[256], DT_data_cnt = 0;
static Data_packge_t Ano_packge ={.buffer = DT_RxBuffer, .len = &DT_data_cnt};

static void ANO_Data_Analyze(uint8_t *data, uint8_t len);
Data_packge_t* ANO_Data_Receive(uint8_t data)
{
    static uint8_t data_len = 0, data_cnt = 0;
    static uint8_t RxState;
    switch(RxState)
    {
        case 0:
        {
            if(data == 0xAA)
            {
                RxState = 1;
                DT_RxBuffer[0] = data;
            }
        }return (Data_packge_t*)(0);
        case 1:
        {
            if(data == HW_TYPE || data == HW_ALL)
            {
                RxState = 2;
                DT_RxBuffer[1] = data;
            }
            else
            {
                RxState = 0;
            }
        }return (Data_packge_t*)(0);
        case 2:
        {
            RxState = 3;
            DT_RxBuffer[2] = data;
        }return (Data_packge_t*)(0);
        case 3:
        {
            if(data < 250)
            {
                RxState = 4;
                DT_RxBuffer[3] = data;
                data_len = data;
                data_cnt = 0;
            }
            else
            {
                RxState = 0;
            }
        }return (Data_packge_t*)(0);
        case 4:
        {
            if( data_len > 0)
            {
                data_len--;
                DT_RxBuffer[4 + data_cnt++] = data;
                if (data_len == 0)RxState = 5;
            }
            else
            {
                RxState = 0;
            }
        }return (Data_packge_t*)(0);
        case 5:
        {
            RxState = 6;
            DT_RxBuffer[4 + data_cnt++] = data;
        }return (Data_packge_t*)(0);
        case 6:
        {
            RxState = 0;
            DT_RxBuffer[4 + data_cnt] = data;
            DT_data_cnt = data_cnt + 5;
            Ano_packge.buffer = DT_RxBuffer;
            Ano_packge.len = &DT_data_cnt;
        }return &Ano_packge;
        default:
        {
            RxState = 0;
        }return (Data_packge_t*)(0);
    }
}

static void ANO_Data_Analyze(uint8_t *dat, uint8_t len)
{
    uint8_t check_SC = 0, check_AC = 0;
    if(*(dat + 3) != (len - 6))return;      //length mismatch
    for (uint8_t i = 0; i < len - 2; i++)
    {
        check_SC += *(dat + i);
        check_AC += check_SC;
    }
    if ((check_SC != *(dat + len - 2)) || (check_AC != *(dat + len - 1)))return; //SC mismatch
    if (*(dat) != 0xAA || (*(dat + 1) != HW_TYPE && *(dat + 1) != HW_ALL))return;//AC mismatch

    switch(*(dat + 2))  //Ano_ID
    {
        case DataVerify://external authentication request
        {

        }break;
        case Imu_adc:
        {

        }break;
        case Compass_baro_temp:
        {

        }break;
        case Euler_angle:
        {

        }break;
        case Quaternion:
        {

        }break;
        case Altitude:
        {

        }break;
        case Ano_CMD:
        {
            switch (*(dat + 4))    //CMD - ID: (*(dat + 4))
            {
                case 0x01:
                {
                     if(*(dat + 5) == 0x00 && *(dat + 6) == 0x03)
                     {
                         IMU_Handle.CaliAcce->calibration_State = Calibration_none;
                     }
                }break;
                case 0x02:
                {
                }break;
                case 0x10:
                {
                }break;
                case 0x11:
                {
                }break;
                default:break;
            }
            ANO_Send.DataVerify(Ano_CMD, check_SC, check_AC);
        }break;
        case Parame_updat:/* shang wei ji read parameter */
        {
            uint16_t P_ID = (uint16_t)(*(dat+4) + (*(dat+5)<<8));//P-ID
            ANO_Send.ParameBack(P_ID);
        }break;
        case Parame_back:/* shang wei ji write parameter */
        {
            uint16_t P_ID = (uint16_t)(*(dat+4) + (*(dat+5)<<8));
            int32_t value = (int32_t)((uint32_t)(*(dat+6))|(*(dat+7)<<8)|(*(dat+8)<<16)|(*(dat+9)<<24));

            ANO_Send.DataVerify(Parame_back, check_SC, check_AC);
            Parameter_update(P_ID, value);
            if(P_ID == 166)
            {
                Parame_FLAG.PID_PSflag = 1;
            }
            //printf("P_ID:%d value:%ld\r\n", P_ID, value);
        }break;
        default:break;
    }
}

/* ANO driver init */
static int ANO_init(void)
{
    Ano_HW.hwtype   = HW_TYPE;
    Ano_HW.hwverion = HW_VER;
    Ano_HW.swverion = SW_VER;
    Ano_HW.blverion = BL_VER;

    ANO_Send.DataVerify = ANO_DataVerify;
    ANO_Send.Inertial = ANO_InertialData_send;
    ANO_Send.CpsBarTem = ANO_CompassBaroTempData_send;
    ANO_Send.EulerAngle = ANO_EulerAngleData_send;
    ANO_Send.Quaternions = ANO_QuaternionsData_send;
    ANO_Send.RemoteData = ANO_RemoteData_send;
    ANO_Send.ParameBack = ANO_ParameBack;
    ANO_Send.Data_analyze = ANO_Data_Analyze;
    ANO_Send.ANO_Data_Receive = ANO_Data_Receive;

    return 0;
}

#include "rtdef.h"
/* RTT support, if RTT isn't available, add
 * ANO_init to your software initialization */
INIT_ENV_EXPORT(ANO_init);
